/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMMTools
* @author     Christian Mandery
* @copyright  2015 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#include <MMM/MathTools.h>
#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/RapidXML/rapidxml.hpp>
#include <MMMSimoxTools/MMMSimoxTools.h>

#include "NloptConverter.h"

using namespace MMM;

NloptConverter::NloptConverter(const std::string &name) :
    MarkerBasedConverter(name),
    optimizationStrategy(FRAMEWISE_LOCAL),
    nloptAlgorithm(nlopt::LN_SBPLX),
    maxFrames(0),
    objectiveFunctionDebugNFrames(0),
    objectiveFunctionEvalCounter(0) {}

bool NloptConverter::setup(ModelPtr inputModel, AbstractMotionPtr inputMotion, ModelPtr outputModel) {
    inputMarkerMotion = boost::dynamic_pointer_cast<MarkerMotion>(inputMotion);

    if (inputMarkerMotion) {
        if (inputModel) {
            MMM_ERROR << "NloptConverter: MarkerMotion given, but also input model!" << std::endl;
            return false;
        }
    } else if (boost::dynamic_pointer_cast<LegacyMotion>(inputMotion)) {
        if (inputModel) {
            inputRobot = SimoxTools::buildModel(inputModel, false);
            if (!inputModel) {
                MMM_ERROR << "NloptConverter: Could not build Simox robot for input model!" << std::endl;
                return false;
            }

            markerScaleFactor = mTargetModelSize / mSourceModelSize;
            MMM_INFO << "NloptConverter: Marker scale factor is " << markerScaleFactor << "." << std::endl;
        } else {
            MMM_ERROR << "NloptConverter: Motion given, but no input model!" << std::endl;
            return false;
        }
    } else {
        MMM_ERROR << "NloptConverter: Unknown type of input motion!" << std::endl;
        return false;
    }

    outputRobot = SimoxTools::buildModel(outputModel, false);
    if (!outputRobot) {
        MMM_ERROR << "NloptConverter: Could not build Simox robot for output model!" << std::endl;
        return false;
    }

    // Improve performance
    outputRobot = SimoxTools::buildReducedModel(outputRobot, jointOrder);
    outputRobot->setUpdateCollisionModel(false);
    outputRobot->setUpdateVisualization(false);
    outputRobot->setupVisualization(false, false);
    outputRobot->setThreadsafe(false);

    if (inputRobot) {
        inputRobot->setUpdateCollisionModel(false);
        inputRobot->setUpdateVisualization(false);
        inputRobot->setupVisualization(false, false);
        inputRobot->setThreadsafe(false);
    }

    for (std::map<std::string, std::string>::iterator i = markerMapping.begin(); i != markerMapping.end(); ++i) {
        inputMarkersToOutputRobotSensors[i->first] = outputRobot->getSensor(i->second);
    }

    return Converter::setup(inputModel, inputMotion, outputModel);
}

AbstractMotionPtr NloptConverter::convertMotion() {
    MMM_INFO << "NloptConverter: convertMotion()" << std::endl;

    switch (optimizationStrategy) {
    case FRAMEWISE_LOCAL: {
        // explicit scope necessary for convertedMotion
        AbstractMotionPtr convertedMotion = initializeStepwiseConversion();

        while (convertedMotion->getNumFrames() < inputMotion->getNumFrames()) {
            if (maxFrames && convertedMotion->getNumFrames() >= maxFrames)
                break;

            if (!convertMotionStep(convertedMotion, true)) {
                MMM_ERROR << "Failed to convert motion step " << convertedMotion->getNumFrames() << std::endl;
                return AbstractMotionPtr();
            }
        }

        return convertedMotion;
    }

    case WHOLE_MOTION:
        return convertMotionWholeMotion();

    default:
        MMM_ERROR << "NloptConverter: Unsupported optimization strategy! (ignored)" << std::endl;
        return AbstractMotionPtr();
    }
}

AbstractMotionPtr NloptConverter::initializeStepwiseConversion() {
    MMM_INFO << "NloptConverter: initializeStepwiseConvertion()" << std::endl;

    LegacyMotionPtr outputMotion(new LegacyMotion(outputModel->getName()));
    outputMotion->setJointOrder(jointOrder);

    // Initialize (unused) initialModelPose and initialJointValues variables
    // These variables are only needed by the MMMConverterGUI and not used for the actual conversion process
    // (in the long run, these members should be removed from the Converter base class
    initialModelPose = Eigen::Matrix4f::Identity();
    initialJointValues.clear();

    for (std::vector<std::string>::const_iterator i = jointOrder.begin(); i != jointOrder.end(); ++i) {
        initialJointValues[*i] = 0.0f;
    }

    return outputMotion;
}

bool NloptConverter::convertMotionStep(AbstractMotionPtr currentOutput, bool increment) {
    MMM_INFO << "NloptConverter: convertMotionStep()" << std::endl;

    if (optimizationStrategy != FRAMEWISE_LOCAL) {
        MMM_ERROR << "NloptConverter: Unsupported optimization strategy! (ignored)" << std::endl;
        return false;
    }

    LegacyMotionPtr outputMotion = boost::dynamic_pointer_cast<LegacyMotion>(currentOutput);
    if (!outputMotion) {
        MMM_ERROR << "NloptConverter: Expecting an currentOutput of type Motion!" << std::endl;
        return false;
    }

    const unsigned int dim = 6 + jointOrder.size();
    const unsigned int inputFrameNum = increment ? outputMotion->getNumFrames() : std::max((int)outputMotion->getNumFrames() - 1, 0);

    if (inputFrameNum >= inputMotion->getNumFrames()) {
        MMM_ERROR << "NloptConverter: End of motion reached!" << std::endl;
        return false;
    }

    curFrame = getInputMarkerData(inputFrameNum);

    // Build initial configuration for optimization
    std::vector<double> configuration;
    if ((increment && inputFrameNum) || (!increment && outputMotion->getNumFrames())) {
        MotionFramePtr lastOutputFrame = outputMotion->getMotionFrame(increment ? inputFrameNum - 1 : inputFrameNum);
        if (!lastOutputFrame) {
            MMM_ERROR << "NloptConverter: Could not load last output frame!" << std::endl;
            return false;
        }

        Eigen::Vector3f rootPos = lastOutputFrame->getRootPos();
        configuration.push_back(rootPos[0]); configuration.push_back(rootPos[1]); configuration.push_back(rootPos[2]);

        Eigen::Vector3f rootRot = lastOutputFrame->getRootRot();
        configuration.push_back(rootRot[0]); configuration.push_back(rootRot[1]); configuration.push_back(rootRot[2]);

        for (int i = 0; i < lastOutputFrame->joint.rows(); ++i) {
            configuration.push_back(lastOutputFrame->joint[i]);
        }
    } else {
        configuration = std::vector<double>(dim, 0.0);

        // Set joints to minimum value if minimum value is higher than zero, or to maximum value if maximum value is lower than zero
        for (unsigned int i = 0; i < jointOrder.size(); ++i) {
            JointInfo jointInfo = outputModel->getModelNode(jointOrder[i])->joint;
            float value = 0.0f;

            if (jointInfo.limitLo > 0)
                value = jointInfo.limitLo;
            if (jointInfo.limitHi < 0)
                value = jointInfo.limitHi;

            configuration[6 + i] = value;
        }
    }

    // Initialize optimization
    objectiveFunctionEvalCounter = 0;
    nlopt::opt optimizer(nloptAlgorithm, dim);
    optimizer.set_min_objective(objectiveFunctionWrapperStatic, this);
    optimizer.set_ftol_abs(0.0001);

    setOptimizationBoundsFramewiseLocal(optimizer);

    // Run optimization
    MMM_INFO << "Starting optimization for frame " << inputFrameNum << "..." << std::endl;
    double objectiveValue;
    try {
        nlopt::result resultCode = optimizer.optimize(configuration, objectiveValue);
        MMM_INFO << "Optimization for frame " << inputFrameNum << " finished with code " << resultCode << ". " <<
                    objectiveFunctionEvalCounter << " calls to objective function." << std::endl;
    }
    catch (const nlopt::roundoff_limited &) {
        MMM_INFO << "Optimization for frame " << inputFrameNum << " finished by throwing nlopt::roundoff_limited (the result should be usable). " <<
                    objectiveFunctionEvalCounter << " calls to objective function." << std::endl;
    }

    // Create and save resulting output motion frame
    MotionFramePtr newOutputFrame;

    if (increment || !outputMotion->getNumFrames()) {
        newOutputFrame.reset(new MotionFrame(jointOrder.size()));
        outputMotion->addMotionFrame(newOutputFrame);
    } else
        newOutputFrame = outputMotion->getMotionFrame(inputFrameNum);

    newOutputFrame->timestep = curFrame->timestamp;

    Eigen::Vector3f rootPos;
    rootPos[0] = configuration[0]; rootPos[1] = configuration[1]; rootPos[2] = configuration[2];
    newOutputFrame->setRootPos(rootPos);

    Eigen::Vector3f rootRot;
    rootRot[0] = configuration[3]; rootRot[1] = configuration[4]; rootRot[2] = configuration[5];
    if (rootRot[0] > M_PI) rootRot[0] -= 2 * M_PI;
    if (rootRot[0] < -M_PI) rootRot[0] += 2 * M_PI;
    if (rootRot[1] > M_PI) rootRot[1] -= 2 * M_PI;
    if (rootRot[1] < -M_PI) rootRot[1] += 2 * M_PI;
    if (rootRot[2] > M_PI) rootRot[2] -= 2 * M_PI;
    if (rootRot[2] < -M_PI) rootRot[2] += 2 * M_PI;
    newOutputFrame->setRootRot(rootRot);

    Eigen::VectorXf jointValues(jointOrder.size());
    for (int i = 0; i < jointValues.rows(); ++i) {
        jointValues[i] = configuration[6 + i];
    }

    newOutputFrame->joint = jointValues;

    // Output error (without CONVERTER_OUTPUT_MARKER_DEVIATION- console output actually is not that expensive!)
    setOutputModelConfiguration(configuration);

    double avgDistance, maxDistance;
    calculateMarkerDistancesAverageMaximum(curFrame, avgDistance, maxDistance);

    std::cout << "Frame #" << inputFrameNum << " finished: max error = " << maxDistance << ", avg error = " << avgDistance << std::endl;

    return true;
}

bool NloptConverter::_setup(rapidxml::xml_node<char> *rootTag) {
    MMM_INFO << "NloptConverter: Reading XML config..." << std::endl;

    // This implementation is partly copied from ConverterVicon2MMM (though simplified)
    rapidxml::xml_node<> *configNode = 0;
    std::string expectedName = name;

    for (rapidxml::xml_node<> *node = rootTag; node; node = node->next_sibling()) {
        std::string nodeName = XML::toLowerCase(node->name());
        if (nodeName == "converterconfig") {
            rapidxml::xml_attribute<> *attr = node->first_attribute("type", 0, false);
            if (!attr) {
                MMM_ERROR << "ConverterConfig: Missing attribute 'type'!" << std::endl;
                return false;
            }

            std::string jn = attr->value();
            if (jn.empty()) {
                MMM_ERROR << "ConverterConfig: Null type string!" << std::endl;
                return false;
            }

            XML::toLowerCase(jn);
            XML::toLowerCase(expectedName);

            if (jn != expectedName) {
                MMM_ERROR << "ConverterConfig type: Expecting " << expectedName << ", got " << jn << std::endl;
                return false;
            }

            configNode = node;
            break;
        }
    }

    if (!configNode) {
        MMM_ERROR << "Could not find ConverterConfig tag!" << std::endl;
        return false;
    }

    rapidxml::xml_node<> *modelNode = configNode->first_node("model", 0, false);
    if (!modelNode) {
        MMM_ERROR << "Could not find Model tag!" << endl;
        return false;
    }
    mTargetModelSize = XML::getOptionalFloatByAttributeName(modelNode, "size", mTargetModelSize);

    if (!readModelConfigXml(modelNode)) {
        return false;
    }

    rapidxml::xml_node<> *optimizationNode = configNode->first_node("optimization", 0, false);
    if (optimizationNode) {
        if (!readOptimizationConfigXml(optimizationNode)) {
            return false;
        }
    }

    return true;
}

AbstractMotionPtr NloptConverter::convertMotionWholeMotion() {
    MMM_INFO << "NloptConverter: convertMotionWholeMotion()" << std::endl;

    const unsigned int frameCount = maxFrames ? (std::min(maxFrames, inputMotion->getNumFrames())) : inputMotion->getNumFrames();
    const unsigned int frameDim = 6 + jointOrder.size();
    const unsigned int dim = frameDim * frameCount;

    for (unsigned int frameNum = 0; frameNum < frameCount; ++frameNum) {
        allFrames.push_back(getInputMarkerData(frameNum));
    }

    // Build initial configuration for optimization
    std::vector<double> configuration(dim, 0.0);

    // Initialize optimization
    objectiveFunctionEvalCounter = 0;
    nlopt::opt optimizer(nloptAlgorithm, dim);
    optimizer.set_min_objective(objectiveFunctionWrapperStatic, this);
    optimizer.set_ftol_abs(0.0001);

    setOptimizationBoundsWholeMotion(optimizer);

    // Run optimization
    MMM_INFO << "Starting optimization of whole motion (" << frameCount << " frames)..." << std::endl;
    double objectiveValue;
    try {
        nlopt::result resultCode = optimizer.optimize(configuration, objectiveValue);
        MMM_INFO << "Optimization finished with code " << resultCode << "." << std::endl;
    }
    catch (const nlopt::roundoff_limited &) {
        MMM_INFO << "Optimization finished by throwing nlopt::roundoff_limited (the result should be usable)." << std::endl;
    }

    // Create resulting output motion
    LegacyMotionPtr outputMotion(new LegacyMotion(outputModel->getName()));
    outputMotion->setJointOrder(jointOrder);

    Eigen::Vector3f rootPos = Eigen::Vector3f::Zero();
    Eigen::Vector3f rootRot = Eigen::Vector3f::Zero();

    for (unsigned int frameNum = 0; frameNum < frameCount; ++frameNum) {
        std::vector<double> frameConfiguration(configuration.begin() + frameNum * frameDim, configuration.begin() + (frameNum + 1) * frameDim);

        MotionFramePtr motionFrame(new MotionFrame(jointOrder.size()));
        motionFrame->timestep = allFrames[frameNum]->timestamp;

        rootPos[0] += frameConfiguration[0]; rootPos[1] += frameConfiguration[1]; rootPos[2] += frameConfiguration[2];
        motionFrame->setRootPos(rootPos);

        rootRot[0] += frameConfiguration[3]; rootRot[1] += frameConfiguration[4]; rootRot[2] += frameConfiguration[5];
        if (rootRot[0] > M_PI) rootRot[0] -= 2 * M_PI;
        if (rootRot[0] < -M_PI) rootRot[0] += 2 * M_PI;
        if (rootRot[1] > M_PI) rootRot[1] -= 2 * M_PI;
        if (rootRot[1] < -M_PI) rootRot[1] += 2 * M_PI;
        if (rootRot[2] > M_PI) rootRot[2] -= 2 * M_PI;
        if (rootRot[2] < -M_PI) rootRot[2] += 2 * M_PI;
        motionFrame->setRootRot(rootRot);

        Eigen::VectorXf jointValues(jointOrder.size());
        for (int i = 0; i < jointValues.rows(); ++i) {
            jointValues[i] = frameConfiguration[6 + i];
        }

        motionFrame->joint = jointValues;

        outputMotion->addMotionFrame(motionFrame);
    }

    return outputMotion;
}

bool NloptConverter::readModelConfigXml(rapidxml::xml_node<char>* modelConfigNode) {
    std::vector<std::string> jointList;

    mTargetModelSize = XML::getOptionalFloatByAttributeName(modelConfigNode, "size", mTargetModelSize);

    for (rapidxml::xml_node<> *node = modelConfigNode->first_node(); node; node = node->next_sibling()) {
        std::string nodeName = XML::toLowerCase(node->name());
        if (nodeName == "markermapping") {
            for (rapidxml::xml_node<> *subNode = node->first_node(); subNode; subNode = subNode->next_sibling()) {
                std::string subNodeName = XML::toLowerCase(subNode->name());
                if (subNodeName == "mapping") {
                    // TODO: We should eventually rename the XML attributes "c3d" and "mmm"
                    rapidxml::xml_attribute<> *inputAttr = subNode->first_attribute("c3d", 0, false);
                    rapidxml::xml_attribute<> *outputAttr = subNode->first_attribute("mmm", 0, false);
                    if (!inputAttr) {
                        MMM_ERROR << "Mapping: Missing attribute 'C3D'!" << std::endl;
                    }
                    else if (!outputAttr) {
                        MMM_ERROR << "Mapping: Missing attribute 'MMM'!" << std::endl;
                    }
                    else {
                        std::string inputMarkerName = inputAttr->value();
                        std::string outputMarkerName = outputAttr->value();
                        if (inputMarkerName.empty() || outputMarkerName.empty()) {
                            MMM_ERROR << "Mapping: Empty marker names not allowed!" << std::endl;
                        } else {
                            if (markerMapping.find(inputMarkerName) != markerMapping.end()) {
                                MMM_ERROR << "Mapping: Input marker name " << inputMarkerName << " is mapped twice!" << std::endl;
                                return false;
                            }

                            for (std::map<std::string, std::string>::const_iterator i = markerMapping.begin(); i != markerMapping.end(); ++i) {
                                if (i->second == outputMarkerName) {
                                    MMM_ERROR << "Mapping: Output marker name " << outputMarkerName << " is mapped twice!" << std::endl;
                                    return false;
                                }
                            }

                            if (inputMarkerMotion && !inputMarkerMotion->hasMarkerLabel(inputMarkerName)) {
                                MMM_ERROR << "Mapping: Input marker name " << inputMarkerName << " is invalid!" << std::endl;
                                return false;
                            } else if (inputModel && !inputModel->hasMarker(inputMarkerName)) {
                                MMM_ERROR << "Mapping: Input marker name " << inputMarkerName << "is invalid!" << std::endl;
                                return false;
                            }

                            if (outputModel && !outputModel->hasMarker(outputMarkerName)) {
                                MMM_ERROR << "Mapping: Output marker name " << outputMarkerName << " is invalid!" << std::endl;
                                return false;
                            }

                            markerMapping[inputMarkerName] = outputMarkerName;
                        }
                    }
                }
                else {
                    MMM_ERROR << "MarkerMapping: Ignoring unknown XML tag " << subNodeName << std::endl;
                }
            }
        } else if (nodeName == "jointset") {
            for (rapidxml::xml_node<> *subNode = node->first_node(); subNode; subNode = subNode->next_sibling()) {
                std::string subNodeName = XML::toLowerCase(subNode->name());
                if (subNodeName == "joint") {
                    rapidxml::xml_attribute<> *nameAttr = subNode->first_attribute("name", 0, false);

                    if (nameAttr) {
                        jointList.push_back(nameAttr->value());
                    } else {
                        MMM_ERROR << "Joint: Missing attribute 'name'!" << std::endl;
                    }
                } else {
                    MMM_ERROR << "JointSet: Ignoring unknown XML tag " << subNodeName << std::endl;
                }
            }
        } else {
            MMM_ERROR << "Model: Ignoring unknown XML tag " << nodeName << std::endl;
        }
    }

    setupJointOrder(jointList);
    for (std::vector<std::string>::const_iterator i = jointList.begin(); i != jointList.end(); ++i) {
        jointValuesToSet[*i] = 0.0f;
    }

    return true;
}

bool NloptConverter::readOptimizationConfigXml(rapidxml::xml_node<char>* optimizationConfigNode) {
    MMM_WARNING << "*** WARNING:" << std::endl;
    MMM_WARNING << "*** You are using the <Optimization> tag to change the default converter settings." << std::endl;
    MMM_WARNING << "*** Changing the settings can lead to reductions in performance and/or output motion quality." << std::endl;

    for (rapidxml::xml_node<> *node = optimizationConfigNode->first_node(); node; node = node->next_sibling()) {
        std::string nodeName = XML::toLowerCase(node->name());
        if (nodeName == "algorithm") {
            rapidxml::xml_attribute<> *nameAttr = node->first_attribute("name", 0, false);

            if (nameAttr) {
                std::string algorithmName = nameAttr->value();

                if (algorithmName == "DIRECT-L") {
                    // Global optimization algorithm
                    // Does not seem to work or converges very slow
                    nloptAlgorithm = nlopt::GN_DIRECT_L;
                } else if (algorithmName == "COBYLA") {
                    // Derivative-free local optimization algorithm
                    // Does not seem to work or converges very slow
                    nloptAlgorithm = nlopt::LN_COBYLA;
                } else if (algorithmName == "BOBYQA") {
                    // Derivative-free local optimization algorithm
                    // Works and converges fast, but segfaults for some motions (needs debugging!)
                    nloptAlgorithm = nlopt::LN_BOBYQA;
                } else if (algorithmName == "SBPLX") {
                    // Derivative-free local optimization algorithm
                    // Slower than BOBYQA, but does not segfault and sometimes produces slightly better results
                    nloptAlgorithm = nlopt::LN_SBPLX;
                } else {
                    MMM_ERROR << "Algorithm: Unknown algorithm name!" << std::endl;
                }
            } else {
                MMM_ERROR << "Algorithm: Missing attribute 'name'!" << std::endl;
            }
        } else if (nodeName == "strategy") {
            rapidxml::xml_attribute<> *nameAttr = node->first_attribute("name", 0, false);

            if (nameAttr) {
                std::string algorithmName = nameAttr->value();

                if (algorithmName == "FramewiseLocal") {
                    optimizationStrategy = FRAMEWISE_LOCAL;
                } else if (algorithmName == "WholeMotion") {
                    optimizationStrategy = WHOLE_MOTION;
                } else {
                    MMM_ERROR << "Strategy: Unknown strategy name!" << std::endl;
                }
            } else {
                MMM_ERROR << "Strategy: Missing attribute 'name'!" << std::endl;
            }
        } else {
            MMM_ERROR << "Optimization: Ignoring unknown XML tag " << nodeName << std::endl;
        }
    }

    return true;
}

LegacyMarkerDataPtr NloptConverter::getInputMarkerData(unsigned int frameNum) const {
    if (inputMarkerMotion) {
        LegacyMarkerDataPtr markerData = inputMarkerMotion->getFrame(frameNum);
        if (!markerData) {
            MMM_ERROR << "NloptConverter: Could not load frame " << frameNum << "!" << std::endl;
            return LegacyMarkerDataPtr();
        }

        return markerData;
    }

    LegacyMotionPtr inputModelMotion = boost::dynamic_pointer_cast<LegacyMotion>(inputMotion);
    if (inputModelMotion) {
        MotionFramePtr motionFrame = inputModelMotion->getMotionFrame(frameNum);

        Eigen::Matrix4f globalPose = Math::poseRPYToMatrix4f(motionFrame->getRootPos(), motionFrame->getRootRot());
        inputRobot->setGlobalPose(globalPose);

        const std::vector<std::string>& jointNames = inputModelMotion->getJointNames();
        for (unsigned int i = 0; i < jointNames.size(); ++i) {
            inputRobot->setJointValue(jointNames[i], motionFrame->joint[i]);
        }

        LegacyMarkerDataPtr markerData(new LegacyMarkerData());
        markerData->frame = frameNum;
        markerData->timestamp = motionFrame->timestep;

        for (std::map<std::string, std::string>::const_iterator i = markerMapping.begin(); i != markerMapping.end(); ++i) {
            const std::string& inputMarker = i->first;
            markerData->data[inputMarker] = inputRobot->getSensor(inputMarker)->getGlobalPose().block(0, 3, 3, 1) * markerScaleFactor;
        }

        return markerData;
    }

    MMM_ERROR << "NloptConverter: Unknown type of input motion!" << std::endl;
    return LegacyMarkerDataPtr();
}

void NloptConverter::setOptimizationBoundsFramewiseLocal(nlopt::opt& optimizer) const {
    // Some algorithms cannot handle unconstraint components (i.e. upper/lower limit of +/- infinity)
    const double positionLowerBound = -10000.0, positionUpperBound = 10000.0;  // 10m

    // We must not limit the rotation strictly at +- pi because otherwise a local optimization algorithm can get stuck when the rotation angle overflows/underflows
    const double rotationOverflowBorder = 0.2;
    const double rotationLowerBound = -M_PI - rotationOverflowBorder, rotationUpperBound = M_PI + rotationOverflowBorder;

    std::vector<double> lowerBounds, upperBounds;

    for (int i = 0; i < 2; ++i) {  // translation & rotation vectors
        for (int j = 0; j < 3; ++j) {
            lowerBounds.push_back(i ? rotationLowerBound : positionLowerBound);
            upperBounds.push_back(i ? rotationUpperBound : positionUpperBound);
        }
    }

    for (std::vector<std::string>::const_iterator i = jointOrder.begin(); i != jointOrder.end(); ++i) {
        JointInfo jointInfo = outputModel->getModelNode(*i)->joint;
        lowerBounds.push_back(jointInfo.limitLo);
        upperBounds.push_back(jointInfo.limitHi);
    }

    optimizer.set_lower_bounds(lowerBounds);
    optimizer.set_upper_bounds(upperBounds);
}

void NloptConverter::setOptimizationBoundsWholeMotion(nlopt::opt& optimizer) const {
    // Some algorithms cannot handle unconstraint components (i.e. upper/lower limit of +/- infinity)
    const double frame0PositionLowerBound = -10000.0, frame0PositionUpperBound = 10000.0;  // 10m
    const double positionMaxChange = 100.0;  // 10cm

    const double frame0RotationLowerBound = -M_PI, frame0RotationUpperBound = M_PI;
    const double rotationMaxChange = 0.2;

    std::vector<double> lowerBounds, upperBounds;

    for (unsigned int frame = 0; frame < allFrames.size(); ++frame) {
        for (int i = 0; i < 2; ++i) {  // translation & rotation vectors
            for (int j = 0; j < 3; ++j) {
                if (frame) {
                    lowerBounds.push_back(i ? -rotationMaxChange : -positionMaxChange);
                    upperBounds.push_back(i ? rotationMaxChange : positionMaxChange);
                } else {
                    lowerBounds.push_back(i ? frame0RotationLowerBound : frame0PositionLowerBound);
                    upperBounds.push_back(i ? frame0RotationUpperBound : frame0PositionUpperBound);
                }
            }
        }

        for (std::vector<std::string>::const_iterator i = jointOrder.begin(); i != jointOrder.end(); ++i) {
            JointInfo jointInfo = outputModel->getModelNode(*i)->joint;
            lowerBounds.push_back(jointInfo.limitLo);
            upperBounds.push_back(jointInfo.limitHi);
        }
    }

    optimizer.set_lower_bounds(lowerBounds);
    optimizer.set_upper_bounds(upperBounds);
}

double NloptConverter::objectiveFunctionWrapperStatic(const std::vector<double> &configuration, std::vector<double> &grad, void *data) {
    return static_cast<NloptConverter*>(data)->objectiveFunctionWrapper(configuration, grad);
}

double NloptConverter::objectiveFunctionWrapper(const std::vector<double> &configuration, std::vector<double> &grad) {
    ++objectiveFunctionEvalCounter;

    bool debugOutput = (objectiveFunctionDebugNFrames && objectiveFunctionEvalCounter % objectiveFunctionDebugNFrames == 0);
    double result;

    switch (optimizationStrategy) {
    case FRAMEWISE_LOCAL:
        result = objectiveFunctionFramewiseLocal(configuration, grad, debugOutput);
        break;

    case WHOLE_MOTION:
        result = objectiveFunctionWholeMotion(configuration, grad, debugOutput);
    }

    if (debugOutput)
        MMM_INFO << "NloptConverter: Objective function evaluated " << objectiveFunctionEvalCounter << " times -> " << result << std::endl;

    return result;
}

double NloptConverter::objectiveFunctionFramewiseLocal(const std::vector<double> &configuration, std::vector<double> &grad, bool debugOutput) {
    if (!grad.empty()) {
        MMM_ERROR << "NloptConverter: Gradient computation not supported!" << std::endl;
        return 0.0;
    }

    const unsigned int dim = 6 + jointOrder.size();
    if (configuration.size() != dim) {
        MMM_ERROR << "NloptConverter: x has wrong number of dimensions (" << configuration.size() << ")!" << std::endl;
        return 0.0;
    }

    setOutputModelConfiguration(configuration);

    return calculateMarkerDistancesSquaresSum(curFrame);
}

double NloptConverter::objectiveFunctionWholeMotion(const std::vector<double> &configuration, std::vector<double> &grad, bool debugOutput) {
    if (!grad.empty()) {
        MMM_ERROR << "NloptConverter: Gradient computation not supported!" << std::endl;
        return 0.0;
    }

    const unsigned int frameDim = 6 + jointOrder.size();
    const unsigned int dim = frameDim * allFrames.size();
    if (configuration.size() != dim) {
        MMM_ERROR << "NloptConverter: x has wrong number of dimensions (" << configuration.size() << ")!" << std::endl;
        return 0.0;
    }

    double totalSumDistanceSquares = 0.0;
    std::vector<double> currentPosRot(6, 0.0);

    for (std::vector<LegacyMarkerDataPtr>::const_iterator curFrameIt = allFrames.begin(); curFrameIt != allFrames.end(); ++curFrameIt) {
        LegacyMarkerDataPtr curFrame = *curFrameIt;

        // Very inefficient
        std::vector<double> frameConfiguration(configuration.begin() + curFrame->frame * frameDim, configuration.begin() + (curFrame->frame + 1) * frameDim);
        for (int i = 0; i < 6; ++i) {
            currentPosRot[i] += frameConfiguration[i];
            frameConfiguration[i] = currentPosRot[i];
        }

        setOutputModelConfiguration(frameConfiguration);

        totalSumDistanceSquares += calculateMarkerDistancesSquaresSum(curFrame);

        /* if (debugOutput)
            MMM_INFO << "NloptConverter: Objective function / frame " << curFrame->frame << " -> " << sumDistanceSquares << std::endl; */
    }

    return totalSumDistanceSquares;
}

void NloptConverter::setOutputModelConfiguration(const std::vector<double> &configuration) {
    Eigen::Vector3f rootPos;
    rootPos[0] = configuration[0]; rootPos[1] = configuration[1]; rootPos[2] = configuration[2];

    Eigen::Vector3f rootRot;
    rootRot[0] = configuration[3]; rootRot[1] = configuration[4]; rootRot[2] = configuration[5];

    Eigen::Matrix4f globalPose = Math::poseRPYToMatrix4f(rootPos, rootRot);
    outputRobot->setGlobalPose(globalPose, false);

    for (unsigned int i = 0; i < jointOrder.size(); ++i) {
        jointValuesToSet[jointOrder[i]] = configuration[6 + i];
    }

    outputRobot->setJointValues(jointValuesToSet);
}

void NloptConverter::calculateMarkerDistancesAverageMaximum(LegacyMarkerDataPtr markerFrame, double &avgDistance, double &maxDistance) const {
    double sumDistances = 0.0;
    maxDistance = 0.0;

    for (std::map<std::string, VirtualRobot::SensorPtr>::const_iterator i = inputMarkersToOutputRobotSensors.begin(); i != inputMarkersToOutputRobotSensors.end(); ++i) {
        std::string inputMarker = i->first;
        VirtualRobot::SensorPtr outputRobotSensor = i->second;

        const Eigen::Vector3f& posInputMarker = markerFrame->data[inputMarker];
        const Eigen::Vector3f& posOutputMarker = outputRobotSensor->getGlobalPose().block(0, 3, 3, 1);

        double distance = (posInputMarker - posOutputMarker).norm();
        sumDistances += distance;

        if (distance > maxDistance) {
            maxDistance = distance;
        }

        /* MMM_INFO << "NloptConverter: Distance between " << c3dMarker << " and " << mmmMarker << ":" << std::endl;
        MMM_INFO << "  C3D: " << posC3D[0] << " " << posC3D[1] << " " << posC3D[2] << std::endl;
        MMM_INFO << "  MMM: " << posMMM[0] << " " << posMMM[1] << " " << posMMM[2] << std::endl;
        MMM_INFO << "  => Distance: " << distance << std::endl; */
    }

    avgDistance = sumDistances / markerMapping.size();
}

double NloptConverter::calculateMarkerDistancesSquaresSum(LegacyMarkerDataPtr markerFrame) const {
    double sumDistancesSquares = 0.0;

    for (std::map<std::string, VirtualRobot::SensorPtr>::const_iterator i = inputMarkersToOutputRobotSensors.begin(); i != inputMarkersToOutputRobotSensors.end(); ++i) {
        std::string inputMarker = i->first;
        VirtualRobot::SensorPtr outputRobotSensor = i->second;

        const Eigen::Vector3f& posInputMarker = markerFrame->data[inputMarker];
        const Eigen::Vector3f& posOutputMarker = outputRobotSensor->getGlobalPose().block(0, 3, 3, 1);

        sumDistancesSquares += (posInputMarker - posOutputMarker).squaredNorm();

        /* MMM_INFO << "NloptConverter: Distance between " << c3dMarker << " and " << mmmMarker << ":" << std::endl;
        MMM_INFO << "  C3D: " << posC3D[0] << " " << posC3D[1] << " " << posC3D[2] << std::endl;
        MMM_INFO << "  MMM: " << posMMM[0] << " " << posMMM[1] << " " << posMMM[2] << std::endl;
        MMM_INFO << "  => Distance: " << distance << std::endl; */
    }

    return sumDistancesSquares;
}
